/***************************************************************************
 创建者: 华磊
 开始时间: 2016.11.28
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 (1)2017.01.19. 华磊 添加TaitBryan"xyz"角正反解，并替换文档Euler"ZYZ"角
 (2)2017.02.20. 华磊 不依赖工具与用户坐标系列表，保障线程安全

华磊，2017.6.6
１）将sixrobot的求解函数变成可重入，在函数内部实例化sixrobot进行使用。
２）测试ＫＤＬ的ik迭代求解方法，证明可行，求解速度快，数值准确。

 ***************************************************************************
 *  @file coordinatesolver.cpp
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#include "coordinatesolver.h"
#include <kdl/frames_io.hpp>
#include <Eigen/Dense>
#include "fiveaxiskinematic.h"

#define BETA
#define IF_DEBUG false

using namespace std;
using namespace KDL;

CoordinateSolver::CoordinateSolver(int robotTypeIn, int internalNumIn, int robotIdIn
                                   , QVector<JointAttribute> *robotAttributeIn,
                                   MotionMessage *motionMessageIn, QVector<double> gravityIn):
    robotType(robotTypeIn),
    internalNum(internalNumIn),
    robotId(robotIdIn),
    robotJoints(robotAttributeIn),
    motionMessage(motionMessageIn)
{

    //debug
    dataRecord = new DataRecord("sixR.csv", 1, 1e5);


    if(3==gravityIn.size())
    {
//        gravityVector=Vector(0.0, 0.0, -9.81);
        gravityVector=Vector(gravityIn[0], gravityIn[1], gravityIn[2]);
    }
    //origin tool mass
    if(robotJoints->size()>0)
    {
        origin_mass=robotJoints->back().mass;
        origin_massCenter=robotJoints->back().massCenter;
    }


    initRobotChain();//robotChain
    jointsMax.resize(internalNum);
    jointsMin.resize(internalNum);
    for (int i=0; i<internalNum; i++)
    {
        if( (*robotJoints)[i].jointType == 0 )//旋转关节
        {
            jointsMax(i) = (*robotJoints)[i].maxPosition * M_PI / 180.0;
            jointsMin(i) = (*robotJoints)[i].minPosition * M_PI / 180.0;
        }
        else//位移关节
        {
            jointsMax(i) = (*robotJoints)[i].maxPosition / 1000.0;
            jointsMin(i) = (*robotJoints)[i].minPosition / 1000.0;
        }
    }//end for (int i=0; i<jointsNum; i++)
    fkSolverPos = new ChainFkSolverPos_recursive(robotChain);
    fkSolverVel = new ChainFkSolverVel_recursive(robotChain);
    ikSolverVel_pinv = new ChainIkSolverVel_pinv(robotChain);
    ikSolverVel_pinvGivens = new ChainIkSolverVel_pinv_givens(robotChain);
    ikSolverPos_LMA = new ChainIkSolverPos_LMA(robotChain);
    ikSolverPos_NR = new ChainIkSolverPos_NR(robotChain, *fkSolverPos, *ikSolverVel_pinv,100, 1e-4);//2017.1.4修改默认迭代次数和误差
    ikSolverPos_NR_JL = new ChainIkSolverPos_NR_JL(robotChain,  jointsMin,  jointsMax, *fkSolverPos, *ikSolverVel_pinv, 20, 1e-4);
    idSolver_RNE = new ChainIdSolver_RNE(robotChain, gravityVector);
    dynParam = new ChainDynParam(robotChain, gravityVector);
//    jointInitial.resize(internalNum);
}

CoordinateSolver::~CoordinateSolver()
{
    if(NULL!=fkSolverPos){
        delete fkSolverPos;
    }
    if(NULL!=fkSolverVel){
        delete fkSolverVel;
    }
    if(NULL!=ikSolverVel_pinv){
        delete ikSolverVel_pinv;
    }
    if(NULL!=ikSolverVel_pinvGivens){
        delete ikSolverVel_pinvGivens;
    }
    if(NULL!=ikSolverPos_LMA){
        delete ikSolverPos_LMA;
    }
    if(NULL!=ikSolverPos_NR){
        delete ikSolverPos_NR;
    }
    if(NULL!=ikSolverPos_NR_JL){
        delete ikSolverPos_NR_JL;
    }
    if(NULL!=idSolver_RNE){
        delete idSolver_RNE;
    }
    if(NULL!=dynParam){
        delete dynParam;
    }
    if(NULL!=dataRecord){
        delete dataRecord;
    }
}

int CoordinateSolver::setGravityVector(vector<double> gravityIn)
{
    qDebug()<<"setGravityVector";
    if(3!=gravityIn.size())
    {
        qDebug()<<"3!=gravityIn.size()";
        return 0;
    }
    gravityVector.data[0]=gravityIn[0];
    gravityVector.data[1]=gravityIn[1];
    gravityVector.data[2]=gravityIn[2];
    delete dynParam;
    dynParam = new ChainDynParam(robotChain, gravityVector);
    return 1;
}

int CoordinateSolver::setToolMass(double mass, vector<double> center)
{
    qDebug()<<"setToolMass"<<mass;
    if(3>center.size())
    {
        qDebug()<<"failed ,setToolMass 3>center.size()";
        return 0;
    }
    (*robotJoints).back().mass=mass+origin_mass;
    (*robotJoints).back().massCenter[0]=(center[0]/1000.0+origin_massCenter[0])/2;
    (*robotJoints).back().massCenter[1]=(center[1]/1000.0+origin_massCenter[1])/2;
    (*robotJoints).back().massCenter[2]=(center[2]/1000.0+origin_massCenter[2])/2;

    Chain robotChainOut;
    initRobotChain_chain(robotChainOut);
    robotChain=robotChainOut;
    delete dynParam;
    dynParam = new ChainDynParam(robotChain, gravityVector);
    return 1;
}

int CoordinateSolver::setSegmentAttribute(int index, JointAttribute attributeIn)
{
    if(index<0 || index>=robotJoints->size())
    {
        return 0;
    }
    qDebug()<<"setSegmentAttribute"<<index;
    (*robotJoints)[index].mass=attributeIn.mass;
    (*robotJoints)[index].massCenter[0]=attributeIn.massCenter[0];
    (*robotJoints)[index].massCenter[1]=attributeIn.massCenter[1];
    (*robotJoints)[index].massCenter[2]=attributeIn.massCenter[2];

    Chain robotChainOut;
    initRobotChain_chain(robotChainOut);
    robotChain=robotChainOut;
    delete dynParam;
    dynParam = new ChainDynParam(robotChain, gravityVector);
    return 1;
}

void CoordinateSolver::initRobotChain()
{
    Chain robotChainOut;
    initRobotChain_chain(robotChainOut);
    robotChain=robotChainOut;

    if(ENUM_6AXIS_CHAIN==robotType || ENUM_DESCART_WRIST_ROBOT==robotType
            )
    {
        if((*robotJoints).size()<6)
        {
            qDebug()<<"error,,,(*robotJoints).size()<6 robotType"<<robotType;
            return ;
        }

        //初始化ＤＨ参数

        for(int i=0;i<6;i++)
        {
            for(int j=0;j<4;j++)
            {
               robotDhParameter[i][j]= (*robotJoints)[i].dhParameter[j];
            }
        }

    }
    else if(ENUM_DESCART_WRAC_ROBOT==robotType)
    {
        fiveAxisKin=new FiveAxisKinematic;
        qDebug()<<"error,,,ENUM_DESCART_WRAC_ROBOT==robotType,need to do,,,"<<robotType;
        return ;
    }
    else if(ENUM_DESCART_WRACTEST_ROBOT==robotType)
    {
        fiveAxisKin=new FiveAxisKinematic;
    }
    else if(ENUM_DELTA_ROBOT_4Axis==robotType)
    {
        if((*robotJoints).size()<4)
        {
            qDebug()<<"error,,,(*robotJoints).size()<4 robotType"<<robotType;
            return ;
        }

        initialDeltaDhParameter(robotJoints);
    }
    else if(ENUM_UR_6AXIS==robotType)
    {
        if((*robotJoints).size()<6)
        {
            qDebug()<<"error,,,(*robotJoints).size()<6 robotType"<<robotType;
            return ;
        }

        initialUrrobotDhParameter(robotJoints);
    }
    else if(ENUM_HEXPOD_6AXIS==robotType)
    {
        if((*robotJoints).size()<6)
        {
            qDebug()<<"error,,,(*robotJoints).size()<6 robotType"<<robotType;
            return ;
        }
        initialHexpodDhParameter(robotJoints);
    }

}

void CoordinateSolver::initRobotChain_chain(Chain &robotChainOut)
{

    Frame dhFrame;
    double a,alpha,d,theta;
//    robotChainOut.addSegment(Segment(Joint(Joint::None),
//									Frame::DH_Craig1989(0.0, 0.0, 0.0, 0.0)));
    for(int i=0; i<internalNum; i++)
    {
        a = (*robotJoints)[i].dhParameter[0];
        alpha = (*robotJoints)[i].dhParameter[1];
        d = (*robotJoints)[i].dhParameter[2];
        theta = (*robotJoints)[i].dhParameter[3];
        RigidBodyInertia rigidBodyInertia=RigidBodyInertia((*robotJoints)[i].mass,
                         Vector((*robotJoints)[i].massCenter[0], (*robotJoints)[i].massCenter[1],
                                (*robotJoints)[i].massCenter[2]),
                         RotationalInertia((*robotJoints)[i].inertiaMatrix[0],(*robotJoints)[i].inertiaMatrix[1],
                         (*robotJoints)[i].inertiaMatrix[2],(*robotJoints)[i].inertiaMatrix[3],
                         (*robotJoints)[i].inertiaMatrix[4],(*robotJoints)[i].inertiaMatrix[5]));
//        if (((*robotJoints)[i].jointType==0)&&((*robotJoints)[i].dhType==1))//旋转关节，modified DH
//        {
//            dhFrame = Frame::DH_Craig1989(a, alpha, d, theta);
//            robotChainOut.addSegment(Segment(Joint(Joint::RotZ), dhFrame,dhFrame.Inverse()*rigidBodyInertia));
//        }
//        else if (((*robotJoints)[i].jointType==1)&&((*robotJoints)[i].dhType==1))//移动关节，modified DH
//        {
//            dhFrame = Frame::DH_Craig1989(a, alpha, d, theta);
//            robotChainOut.addSegment(Segment(Joint(Joint::TransZ), dhFrame,rigidBodyInertia));
//        }
        if (((*robotJoints)[i].jointType==0))//旋转关节，standard DH
        {
            dhFrame = Frame::DH(a, alpha, d, theta);
            robotChainOut.addSegment(Segment(Joint(Joint::RotZ), dhFrame,rigidBodyInertia));
        }
        else//((robotJoints[i].jointType==1)&&(robotJoints[i].dhType==0))//移动关节，standard DH
        {
            dhFrame = Frame::DH(a, alpha, d, theta);
            robotChainOut.addSegment(Segment(Joint(Joint::TransZ), dhFrame,rigidBodyInertia));
        }
    }

    robotChainOut.addSegment(Segment(Joint(Joint::None),
                                       Frame(Rotation::Identity(),Vector(0.0,0.0,0.0))));
}




int CoordinateSolver::pointCoordinateToCartesian(const PointPro pointIn, PointPro &pointOut)
{
    pointOut=pointIn;
    if(pointIn.jointFlag==JOINT_FLAG_CARTESIAN)
    {
        qDebug()<<"pointCoordinateToCartesian warnning, pointIn.jointFlag==JOINT_FLAG_CARTESIAN";

        return 1;
    }
    pointOut.jointFlag = JOINT_FLAG_CARTESIAN;
    pointOut.positionValue.resize(6);
    if(ENUM_DESCART_WRACTEST_ROBOT!=robotType)
    {
        Frame tmpFrame;
        if(1!=pointProToFrame_noUT(pointIn,tmpFrame))
        {
            return -1;
        }

        float v[6];
        double v3,v4,v5;
    //    tmpFrame.M.GetEulerZYZ(v3, v4, v5);
        extractEulerMatrixAngle(tmpFrame.M ,v3, v4, v5);
    //    if(IF_DEBUG)
    //    {
    //        std::cout << "eulerZYZ:" << v3 << " " << v4 << " " << v5 <<std::endl;
    //    }
        v[0] = tmpFrame.p[0]*1000.0; //笛卡尔，三维坐标，m转mm
        v[1] = tmpFrame.p[1]*1000.0;
        v[2] = tmpFrame.p[2]*1000.0;
        v[3] = v3*180.0/M_PI;//笛卡尔，欧拉角，弧度转度
        v[4] = v4*180.0/M_PI;
        v[5] = v5*180.0/M_PI;

        for(int i=0; i<6; i++)
        {
            pointOut.positionValue[i] = v[i];
        }
    }
    else
    {
        fiveAxisKin->kinematicsForward(pointIn.positionValue,pointOut.positionValue);
    }

    vector<double> tmpPosition;
    tmpPosition.resize(pointIn.positionValue.size());
    for(int i=0; i<pointIn.positionValue.size(); i++)
    {
        tmpPosition[i]=pointIn.positionValue[i];
    }
    getMultiConfig(tmpPosition, pointOut.configString, pointOut.configInt);

    //pointAddCoordinate(pointOut);//CoordinateManager::pointCoordinateToCartesian 会添加坐标系
    return 1;
}


int CoordinateSolver::pointCoordinateToJoint(const PointPro pointIn,
                vector<double> positionInitialList, PointPro &jointPointOut, bool isErrorNotice)
{

    if(ENUM_DESCART_WRACTEST_ROBOT!=robotType)
    {
        Frame tmpFrame;
        if(1!=pointProToFrame_noUT(pointIn,tmpFrame))
        {
            return -1;
        }

        //int isSinguraty = coordinateSolver->frameToPointPro_JOINT(f, pointOut, userCoordinateIndex, toolCoordinateIndex);
        jointPointOut=pointIn;

        bool isSinguratyOut;
        return frameToPointPro_JOINT(tmpFrame, jointPointOut,isSinguratyOut, pointIn.configString, positionInitialList
                                     ,isErrorNotice);
    }
    else
    {
        jointPointOut.jointFlag=JOINT_FLAG_JOINT;
        return fiveAxisKin->kinematicsInverse(pointIn.positionValue,jointPointOut.positionValue);
    }

    return 0;

}




Rotation CoordinateSolver::createEulerMatrix(double psi, double sita, double fai)
{
    return Rotation::RPY(psi,sita,fai);

//    double s1 =sin(psi);
//    double c1 =cos(psi);
//    double s2 =sin(sita);
//    double c2 =cos(sita);
//    double s3 =sin(fai);
//    double c3 =cos(fai);
//    return Rotation(c2*c3, -c2*s3, s2,
//                    c1*s3+c3*s1*s2, c1*c3-s1*s2*s3, -c2*s1,
//                    s1*s3-c1*c3*s2, c3*s1+c1*s2*s3, c1*c2);
}

void CoordinateSolver::extractEulerMatrixAngle(const Rotation &rotMat, double &psi, double &sita, double &fai)
{
    rotMat.GetRPY(psi,sita,fai);
    return ;

//    fai = atan2(-rotMat.data[1], rotMat.data[0]);//范围-pi~pi
//    sita = asin(rotMat.data[2]);//范围-pi/2~pi/2
//    psi = atan2(-rotMat.data[5], rotMat.data[8]);//范围-pi~pi
}



int CoordinateSolver::pointProToFrame_noUT(const PointPro &pointPro,Frame &frameOut)
{
    Frame f;
    if(pointPro.jointFlag==JOINT_FLAG_CARTESIAN)
    {
        if(6>pointPro.positionValue.size())
        {
            qDebug()<<"------------error,,,pointProToFrame_noUT 6>pointPro.positionValue.size()";
            return 0;
        }
//        f = Frame(Rotation::EulerZYZ(pointPro.positionValue[3]*M_PI/180.0,
//                        pointPro.positionValue[4]*M_PI/180.0,
//                        pointPro.positionValue[5]*M_PI/180.0),
//                        Vector (pointPro.positionValue[0]/1000.0,
//                            pointPro.positionValue[1]/1000.0,
//                            pointPro.positionValue[2]/1000.0));
        f = Frame(createEulerMatrix(pointPro.positionValue[3]*M_PI/180.0,
                        pointPro.positionValue[4]*M_PI/180.0,
                        pointPro.positionValue[5]*M_PI/180.0),
                        Vector (pointPro.positionValue[0]/1000.0,
                            pointPro.positionValue[1]/1000.0,
                            pointPro.positionValue[2]/1000.0));
    }
    else
    {
        if(pointPro.positionValue.size()!=internalNum)
        {
            qDebug()<<"------------error,,,pointPro.positionValue.size()!=internalNum"<<pointPro.positionValue.size()
                   <<internalNum;
            return -1;
        }
        if(0==robotJoints->size())
        {
            return 0;
        }
        JntArray jointArray(internalNum);
        for(int i=0; i<internalNum; i++)
        {
            if((*robotJoints)[i].jointType==0)//旋转关节
            {
                jointArray(i) = pointPro.positionValue[i]*M_PI/180.0;//hualei todo ,will coredump.j PR[]
            }
            else//移动关节
            {
                jointArray(i) = pointPro.positionValue[i] / 1000.0;
            }
        }
        if(0>getFkPos(jointArray,f))
        {
            return 0;
        }
    }
    frameOut=f;
    return 1;
}

Frame CoordinateSolver::jntArrayToFrame(const JntArray &jntArray)
{
    Frame f;
    assert(getFkPos(jntArray,f)==0);
    return f;
}

PointPro CoordinateSolver::frameToPointPro_CART(const Frame &frameIn)
{
    PointPro pointOut;
    double v[6];
    double v3,v4,v5;
//    f.M.GetEulerZYZ(v3, v4, v5);
    extractEulerMatrixAngle(frameIn.M, v3, v4, v5);
    v[0] = frameIn.p[0]*1000.0; //笛卡尔，三维坐标，m转mm
    v[1] = frameIn.p[1]*1000.0;
    v[2] = frameIn.p[2]*1000.0;
    v[3] = v3*180.0/M_PI;//笛卡尔，欧拉角，弧度转度
    v[4] = v4*180.0/M_PI;
    v[5] = v5*180.0/M_PI;
    pointOut.jointFlag = JOINT_FLAG_CARTESIAN;
    pointOut.ufValue = 0;
    pointOut.utValue = 0;
    pointOut.positionValue.resize(6);
    for(int i=0; i<6; i++)
    {
        pointOut.positionValue[i]=v[i];
    }
    //getMultiConfig(pointOut.positionValue, pointOut.configString, pointOut.configInt);
    return pointOut;
}

int CoordinateSolver::transConfigString2ConfigInt(QString configString, QVector<int>& futConfig)
{

    switch (robotType)
    {
        case ENUM_SCARA:
        {
            futConfig.resize(4);
            QString pattern("\\s*([FNLR]{1}),\\s*([U]?[D]?)\\s*,?\\s*([B]?[T]?)\\s*,?(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(.*)");
            QRegExp rx(pattern);
            configString.indexOf(rx);
            QString capStr1 = rx.cap(1);
            QString capStr2 = rx.cap(2);
            QString capStr3 = rx.cap(3);
            QString capSymb = rx.cap(4);
            QString capNum = rx.cap(5);
            int capValue = capNum.toInt();
            if(capSymb=="-")
            {
                capValue = -capValue;
            }
            futConfig[3] = capValue;
            if(capStr1=="L")
            {
                futConfig[1] = -1;
            }
            else
            {
                futConfig[1] = 1;
            }

            //todo hualei
            futConfig[0] = 0;
            futConfig[1] = 0;


            break;
        }
        case ENUM_UR_6AXIS:
        {
        return transConfigString2ConfigInt_UR(configString,futConfig);
        }
        case ENUM_DESCART_XYZR_ROBOT:
        {
        return transConfigString2ConfigInt_XYZR(configString,futConfig);
        }
        case ENUM_6AXIS_CHAIN:
        {
        return transConfigString2ConfigInt_FANUC(configString,futConfig);
            break;
        }
        case ENUM_4AXIS_PALLET:
        {
           addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7068,robotId);
           break;
        }
        case ENUM_7AXIS:
        {

            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7068,robotId);
            break;
        }
        case ENUM_DELTA_ROBOT_3Axis:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7068,robotId,robotType);
            return -1;
        }
        case ENUM_DELTA_ROBOT_4Axis:
        {
            futConfig.resize(1);
            futConfig[0]=configString.toInt();
            return 1;
//            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7068,robotId,robotType);
//            return -1;
        }
        case ENUM_DELTA_ROBOT_4Axis_EIGHT_LEG:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7068,robotId,robotType);
            return -1;
        }
    case ENUM_HEXPOD_6AXIS:
    case ENUM_DESCART_WRACTEST_ROBOT:
    case ENUM_DESCART_ROBOT:

    {
        return 1;
    }
    case ENUM_DESCART_WRAC_ROBOT:
    case ENUM_DESCART_WRIST_ROBOT:
    {
        futConfig.resize(6);
        QString pattern("\\s*([FNLR]{1}),?(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,?(.*)");
        QRegExp rx(pattern);
        configString.indexOf(rx);
        QString capStr1 = rx.cap(1);
        QString capSymb1 = rx.cap(2);
        QString capNum1 = rx.cap(3);
        QString capSymb2 = rx.cap(4);
        QString capNum2 = rx.cap(5);
        futConfig[1] = capNum1.toInt();
        futConfig[2] = capNum2.toInt();

        if(capSymb1=="-")
        {
            futConfig[1] = -futConfig[1];
        }
        if(capSymb2=="-")
        {
            futConfig[2] = -futConfig[2];
        }

        if (capStr1 == "F")
        {
            futConfig[0] = 1;
        }
        else
        {
            futConfig[0] = 0;
        }


        break;
    }
        default:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7068,robotId);
            if(IF_DEBUG)
            {
                qDebug()<<"CoordinateSolver::frameToPointPro_JOINT====程序bug,不支持机器人类型"<< robotType;
            }
            return -1;
        }
    }

    return 1;

}

int CoordinateSolver::transConfigString2ConfigInt_UR(QString configString,
                                                     QVector<int> &futConfig)
{
    QString pattern("\\s*(\\-?\\d*),(\\-?\\d*),(\\-?\\d*),(\\-?\\d*),(\\-?\\d*),(\\-?\\d*),\\s*");
    QRegExp rx(pattern);
    configString.indexOf(rx);
    for(int i=1;i<7;i++)
    {
        futConfig.append(rx.cap(i).toInt());
    }

    return 1;

}

int CoordinateSolver::transConfigString2ConfigInt_XYZR(QString configString, QVector<int> &futConfig)
{
    QString pattern("\\s*(\\-?\\d*)\\s*");
    QRegExp rx(pattern);
    configString.indexOf(rx);
    for(int i=1;i<2;i++)
    {
        futConfig.append(rx.cap(i).toInt());
    }

    return 1;
}

int CoordinateSolver::transConfigString2ConfigInt_FANUC(QString configString, QVector<int> &futConfig)
{
    futConfig.resize(6);//\\s*
//    QString pattern(".*([FNLR]{1}),\\s*([U]?[D]?)\\s*,?\\s*([B]?[T]?)\\s*,?(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,?(\\-?)(\\d?)\\s*,?(\\-?)(\\d?)\\s*,?(\\-?)(\\d?)\\s*,?(.*)");
    QString pattern(".*([FNLR]{1})\\s*([U]?[D]?)\\s*([B]?[T]?),\\s*(\\-?)(\\d*),\\s*(\\-?)(\\d*),\\s*(\\-?)(\\d*).*");
    QRegExp rx(pattern);
    configString.indexOf(rx);
    QString capStr1 = rx.cap(1);
    QString capStr2 = rx.cap(2);
    QString capStr3 = rx.cap(3);
    QString capSymb1 = rx.cap(4);
    QString capNum1 = rx.cap(5);
    QString capSymb2 = rx.cap(6);
    QString capNum2 = rx.cap(7);
    QString capSymb3 = rx.cap(8);
    QString capNum3 = rx.cap(9);
    futConfig[3] = capNum1.toInt();
    futConfig[4] = capNum2.toInt();
    futConfig[5] = capNum3.toInt();
    if(capSymb1=="-")
    {
        futConfig[3] = -futConfig[3];
    }
    if(capSymb2=="-")
    {
        futConfig[4] = -futConfig[4];
    }
    if(capSymb3=="-")
    {
        futConfig[5] = -futConfig[5];
    }
    if (capStr1 == "F")
    {
        futConfig[0] = 1;
    }
    else
    {
        futConfig[0] = 0;
    }
    if (capStr2 == "U")
    {
        futConfig[1] = 1;
    }
    else
    {
        futConfig[1] = 0;
    }
    if (capStr3 == "T")
    {
        futConfig[2] = 1;
    }
    else
    {
        futConfig[2] = 0;
    }
    return 1;
}

int CoordinateSolver::frameToPointPro_JOINT(const Frame& frameIn,  PointPro& pointOut,bool &isSinguratyOut
              , string configString, vector<double> positionInitialList, bool isErrorNotice)
{
    isSinguratyOut=false;
//    usingMutex.lock();
    JntArray jointsOut(internalNum);

    if(0==positionInitialList.size())
    {

        if(0==configString.size())
        {

            qDebug() << "error CoordinateSolver::frameToPointPro_JOINT==== 0==configString.size()";
            return -1;
        }
        QString tmpString = QString::fromStdString(configString);
        QVector<int> futConfig;
        transConfigString2ConfigInt(tmpString, futConfig);
        //快速几何法，fut
        if (getIkPos_quick_fut(futConfig, frameIn, jointsOut,isSinguratyOut) < 0)
        {
            if(isErrorNotice)
            {
                addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7060,robotId);
            }

            if(IF_DEBUG)
            {
                qDebug() << "CoordinateSolver::frameToPointPro_JOINT====7轴坐标转换失败";
            }
//            usingMutex.unlock();
            return -1;
        }
    }
    else
    {
        vector<double> tmpDeb=positionInitialList;
        if(1!=mmDegree2mRad(positionInitialList,positionInitialList))
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7071,robotId);
            return -1;
        }
        JntArray jointInitial(internalNum);
        for(int i=0;i<internalNum;i++)
        {
                jointInitial(i)=positionInitialList[i];
        }
        //迭代法,快速几何法选择
        if (getIkPos_quick(jointInitial, frameIn, jointsOut,isSinguratyOut) < 0)
        {
            if(isErrorNotice)
            {
                addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7060,robotId);
            }
            if(IF_DEBUG)
            {
                qDebug() << "CoordinateSolver::frameToPointPro_JOINT====7轴坐标转换失败";
            }
            mmDegree2mRad(tmpDeb,tmpDeb);
//            usingMutex.unlock();
            return -1;
        }

    }


    pointOut.jointFlag = JOINT_FLAG_JOINT;
    pointOut.ufValue = 0;
    pointOut.utValue = 0;
    pointOut.positionValue.resize(internalNum);
    pointOut.configString.clear();
    pointOut.configInt.clear();
    mRad2mmDegree(jointsOut,pointOut.positionValue);

//    usingMutex.unlock();
    return 1;
}

int CoordinateSolver::frameToPointPro_JOINT(const Frame &frameIn, PointPro &pointOut, string configString)
{
    vector<double> positionInitialList;
    bool isSinguratyOut;
    return frameToPointPro_JOINT(frameIn,pointOut,isSinguratyOut,configString,positionInitialList);
}

int CoordinateSolver::getIdealJointTorque(const JntArray& q_in,  JntArray tmpQdot,JntArray tmpQdotdot,
                                          Wrench f_tool,JntArray &torqueOut)
{


    int getFkPosReturn=-1;
    switch(robotType)
    {

    case ENUM_UR_6AXIS:
    {

        if(1)
        {
            JntArray tmpQIn=q_in;
            tmpQIn(1)=tmpQIn(1)-M_PI_2;
            tmpQIn(3)=tmpQIn(3)-M_PI_2;

            for(int i=0;i<6;i++)
            {
                tmpQdot(0)=0;
                tmpQdotdot(0)=0;
                torqueOut(0)=0;
            }
            unsigned int ns = robotChain.getNrOfSegments();

            // In local link coordinates
            Wrenches f_ext(ns);
            for(unsigned int i=0;i<ns;i++)
            {
                SetToZero(f_ext[i]);
            }
            f_ext[ns-1]=f_tool;

//            getIdSolver_RNE(tmpQIn,tmpQdot,tmpQdotdot,f_ext,torqueOut);
//            if(torqueOut.rows()>0)
//            {
//                qDebug()<<"getIdSolver_RNE:ideal torque"<<torqueOut(0)<<torqueOut(1)<<torqueOut(2)
//                       <<torqueOut(3)<<torqueOut(4)<<torqueOut(5);
//            }
            getGravity(tmpQIn,torqueOut);
            if(torqueOut.rows()>0)
            {
//                qDebug()<<"getGravity:ideal torque"<<torqueOut(0)<<torqueOut(1)<<torqueOut(2)
//                       <<torqueOut(3)<<torqueOut(4)<<torqueOut(5);
            }
            return 1;

        }


    }
    break;
//    case ENUM_SCARA:
//    {

//    }
//    break;
//    case ENUM_4AXIS_PALLET:
//    {

//    }
//    break;
//    case ENUM_DESCART_WRIST_ROBOT:
//    case ENUM_6AXIS_CHAIN:
//    {


//    }
//    break;
//    case ENUM_7AXIS:
//    {

//    }
//    break;
//    case ENUM_DELTA_ROBOT_3Axis:
//    {

//    }
//    break;
//    case ENUM_DELTA_ROBOT_4Axis:
//    {

//    }
//    break;
//    case ENUM_HEXPOD_6AXIS:
//    {


//    }
//    break;
//    case ENUM_DELTA_ROBOT_4Axis_EIGHT_LEG:
//    {

//    }
//    break;

        default:
        {
            qDebug()<<"getIdealJointTorque,unsupport  robotType"<<robotType;
           return 0;
        }
    }
    return getFkPosReturn;
}

int CoordinateSolver::getTurnsFromDegree(double degreeIn)
{
//    int turns;
//    degreeIn = (degreeIn+180.0) /360.0;
//    if(degreeIn>0.0){
//        turns = floor(degreeIn);//往下取整
//    }
//    else{
//        turns = ceil(degreeIn);//往上取整
//    }

    return getTurnsFromRad(degreeIn/180.0*M_PI);
}

int CoordinateSolver::getTurnsFromRad(double degreeIn)
{
    int turns;
//    degreeIn = (degreeIn+M_PI) /(2*M_PI);
//    if(degreeIn>0.0){
//        turns = floor(degreeIn);//往下取整
//    }
//    else{
//        turns = ceil(degreeIn);//往上取整
//    }
    if(0<degreeIn)
    {
        turns=(degreeIn+M_PI) /(2*M_PI);
    }
    else
    {
        turns=(degreeIn-M_PI) /(2*M_PI);
    }

    return turns;
}

double CoordinateSolver::getTrimDegreeFromDegree(double degreeIn)
{
    int turns=getTurnsFromDegree(degreeIn);
    double trimDegree=degreeIn-360.0*turns;
    return trimDegree;

}

double CoordinateSolver::getTrimRadFromRad(double degreeIn)
{
    int turns=getTurnsFromRad(degreeIn);
    double trimDegree=degreeIn-2*M_PI*turns;
    return trimDegree;

}

int CoordinateSolver::getBestPositionFromAngle(double idealAngle, double initialPosition, double &bestPositionOut)
{
    int currentTurns;
    if(0<initialPosition)
    {
        currentTurns=(initialPosition+180) /(360);
    }
    else
    {
        currentTurns=(initialPosition-180) /(360);
    }

    double angle1,angle2,angle3;
    angle1=currentTurns*360+idealAngle;
    angle2=angle1+360;
    angle3=angle1-360;
    double offset1,offset2,offset3;
    double minNum;
    int minIndex;
    offset1=fabs(angle1-initialPosition);
    offset2=fabs(angle2-initialPosition);
    offset3=fabs(angle3-initialPosition);
    if(offset1<=offset2)
    {
        minNum=offset1;
        minIndex=1;
    }
    else
    {
        minNum=offset2;
        minIndex=2;
    }

    if(offset3<=minNum)
    {
        minNum=offset3;
        minIndex=3;
    }
    switch(minIndex)
    {
    case 1:
    {
        bestPositionOut=angle1;
        break;

    }
    case 2:
    {
        bestPositionOut=angle2;
        break;

    }
    default:
    {
        bestPositionOut=angle3;
    }
    }


    return 1;
}

int CoordinateSolver::getBestPositionFromAngle_Rad(double idealAngle, double initialPosition, double &bestPositionOut)
{
    int tmpKey= getBestPositionFromAngle( idealAngle/M_PI*180.0, initialPosition/M_PI*180.0, bestPositionOut);
    bestPositionOut=bestPositionOut/180.0*M_PI;
    return tmpKey;
}

int CoordinateSolver::initialDeltaDhParameter(QVector<JointAttribute> *robotAttributeIn)
{

    //dh单位使用m
    if(3>robotAttributeIn->size())
    {
        qDebug()<<"error,initialDeltaDhParameter 3>robotAttributeIn->size()";
        return 0;
    }
//    deltaRobotKinetic=new DeltaKinematics<double>((*robotAttributeIn)[0].dhParameter[3],(*robotAttributeIn)[0].dhParameter[0],
//            (*robotAttributeIn)[0].dhParameter[1],(*robotAttributeIn)[0].dhParameter[2],(*robotAttributeIn)[1].dhParameter[0],
//            (*robotAttributeIn)[1].dhParameter[3],(*robotAttributeIn)[1].dhParameter[1],(*robotAttributeIn)[1].dhParameter[2]);

    deltaRobotKinetic=new DeltaKinematicsKe((*robotAttributeIn)[0].dhParameter[0],
            (*robotAttributeIn)[0].dhParameter[1],
            (*robotAttributeIn)[0].dhParameter[2],
            (*robotAttributeIn)[0].dhParameter[3],
            (*robotAttributeIn)[1].dhParameter[0],
            (*robotAttributeIn)[1].dhParameter[3]);


    return 1;

}

int CoordinateSolver::initialUrrobotDhParameter(QVector<JointAttribute> *robotAttributeIn)
{
    //dh单位使用m
    if(6>robotAttributeIn->size())
    {
        qDebug()<<"error,initialDeltaDhParameter 3>robotAttributeIn->size()";
        return 0;
    }
    // d1, a2, a3, d4, d5, d6
    // a f d j
    urKinematic=new UrKinematics((*robotAttributeIn)[0].dhParameter[2],
            (*robotAttributeIn)[1].dhParameter[0],
            (*robotAttributeIn)[2].dhParameter[0],
            (*robotAttributeIn)[3].dhParameter[2],
            (*robotAttributeIn)[4].dhParameter[2],
            (*robotAttributeIn)[5].dhParameter[2]);


    return 1;
}

int CoordinateSolver::initialHexpodDhParameter(QVector<JointAttribute> *robotAttributeIn)
{
    hexpodKin=new HexPodKinematics();
}

double CoordinateSolver::transformToPi(double angleIn)
{
    //    int turns=angleIn/M_PI/2;//可能bug 参考getTurnsFromRad
        int turns;

        if(0<angleIn)
        {
            turns=(angleIn+M_PI) /(2*M_PI);
        }
        else
        {
            turns=(angleIn-M_PI) /(2*M_PI);
        }

        //    angleIn=angleIn-turns*M_PI;//错误
            angleIn=angleIn-turns*M_PI*2;

        if(angleIn>=-M_PI && angleIn<=M_PI)
        {
            return angleIn;
        }
        else if(angleIn<-M_PI)
        {
            return angleIn+2*M_PI;
        }
        else if(angleIn>M_PI)
        {
            return angleIn-2*M_PI;
        }

        return angleIn;
}

int getTrimRadFromRad(double degreeIn);

int CoordinateSolver::getMultiConfig(const std::vector<double> positionValue,
                                     std::string &configString, std::vector<int> &configInt)
{
    if(positionValue.size()<=0)
    {
        return 0;
    }
    switch(robotType)
    {
        case ENUM_SCARA:
        {
            if(positionValue.size()<4)
            {
                return 0;
            }
            QString tmpStr;
            configInt.resize(4);
             // 左右手
            if(positionValue[1] < 0.0)//轴2判断多解
            {
                tmpStr = "L" ;
                configInt[0] = 0;
            }
            else
            {
                tmpStr = "R" ;
                configInt[0] = 1;
            }


            //多圈
            double tmpDegree;
            for(int i=0;i<4;i++)
            {
                if(i!=2)
                {
                    configInt[i+1] = getTurnsFromDegree(positionValue[i]);
                    tmpStr = tmpStr+","+QString::number(configInt[1]);
                }
            }



            configString = tmpStr.toStdString();
            break;
        }
        case ENUM_UR_6AXIS:
        {
        return getMultiConfig_UR(positionValue, configString, configInt);
        }
        case ENUM_DESCART_XYZR_ROBOT:
        {
        return getMultiConfig_XYZR(positionValue, configString, configInt);
        }
        case ENUM_6AXIS_CHAIN:
        {
        return getMultiConfig_FANUC(positionValue, configString, configInt);

            break;
        }
        case ENUM_4AXIS_PALLET:
        {
//            qDebug()<<"CoordinateSolver::getMultiConfig====程序bug,不支持机器人类型"<<robotType;
            return 1;
            break;
        }
        case ENUM_7AXIS:
        {
            qDebug()<<"CoordinateSolver::getMultiConfig====程序bug,不支持机器人类型"<<robotType;
            return -1;
            break;
        }
    case ENUM_HEXPOD_6AXIS:
    case ENUM_DESCART_ROBOT:
    {
        return 1;
    }
    case ENUM_DESCART_WRACTEST_ROBOT:
    case ENUM_DESCART_WRAC_ROBOT:
    {
        return 1;
    }
    case ENUM_DESCART_WRIST_ROBOT:
    {
        if(positionValue.size()<6)
        {
            return 0;
        }
        configInt.resize(3);

        //判断四、六轴的多圈
        configInt[1] = getTurnsFromDegree(positionValue[3]);
        configInt[2] = getTurnsFromDegree(positionValue[5]);
        QString tmpStr = QString::number(configInt[1]) + "," +QString::number(configInt[2]) ;

        // 判断求解姿态,手腕上下F(N)，机臂上下Ｕ(D)，机臂前后T(B)


        if(positionValue[4]>0)
        {
            configInt[0] = 1;
        }
        else
        {
            configInt[0] = 0;
        }



        if(configInt[0] == 0)//判断手腕上下F(N)
        {
            tmpStr = "N," +tmpStr;
        }
        else
        {
            tmpStr = "F," + tmpStr;
        }
        configString = tmpStr.toStdString();
        break;
    }

    case ENUM_DELTA_ROBOT_4Axis:
    {
        if(positionValue.size()<4)
        {
            return 0;
        }
        //多圈
        double tmpDegree;
        QString tmpStr;
        configInt.resize(1);

        configInt[0] = getTurnsFromDegree(positionValue[3]);
        tmpStr = QString::number(configInt[0]);

        configString = tmpStr.toStdString();
        return 1;
    }
        default:
        {
            if(IF_DEBUG)
            {
                qDebug()<<"CoordinateSolver::getMultiConfig====程序bug,不支持机器人类型"<<robotType;
            }
            return -1;
        }
    }//end switch
    return 1;
}

int CoordinateSolver::getMultiConfig_UR(const std::vector<double> positionValue,
                                        string &configString, std::vector<int> &configInt)
{
    if(positionValue.size()<6)
    {
        return 0;
    }
    configInt.resize(6);

    QString tmpStr;
    for(int i=0;i<6;i++)
    {
        configInt[i]=positionValue[i];
        tmpStr += QString::number(configInt[i]) + ",";
    }
    configString = tmpStr.toStdString();
    return 1;
}

int CoordinateSolver::getMultiConfig_XYZR(const std::vector<double> positionValue, string &configString, std::vector<int> &configInt)
{
    if(positionValue.size()<4)
    {
        return 0;
    }
    configInt.resize(1);

    QString tmpStr;
    for(int i=0;i<1;i++)
    {
        configInt[i]=positionValue[3];
        tmpStr += QString::number(configInt[i]);// + ","
    }
    configString = tmpStr.toStdString();
    return 1;
}

int CoordinateSolver::getMultiConfig_FANUC(const std::vector<double> positionValue, string &configString, std::vector<int> &configInt)
{
        if(positionValue.size()<6)
        {
            return 0;
        }
        configInt.resize(6);

        //判断第一、四、六轴的多圈
        configInt[3] = getTurnsFromDegree(positionValue[0]);
        configInt[4] = getTurnsFromDegree(positionValue[3]);
        configInt[5] = getTurnsFromDegree(positionValue[5]);
        QString tmpStr = QString::number(configInt[3]) + "," +QString::number(configInt[4]) + ","
                +QString::number(configInt[5])+ "'," ;

        // 判断求解姿态,手腕上下F(N)，机臂上下Ｕ(D)，机臂前后T(B)
        int fut[3];
        double jointPosition[6];
        for (int i = 0; i < 6; i++)
        {
            jointPosition[i] = positionValue[i] / 180.0 * M_PI;
        }

        SixJointRobot sixJointRobotSolver;
        sixJointRobotSolver.initialForIk(robotDhParameter);
        int ikConfig = sixJointRobotSolver.getIkConfig(jointPosition, fut);
        if (-1 == ikConfig)
        {
            //addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7061,robotId);
            return -1;
        }
        if (-2 == ikConfig)
        {
            //addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7062,robotId);
            return -1;
        }
        if (-3 == ikConfig)
        {
           // addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7063,robotId);
            return -1;
        }
        for (int i = 0; i < 3; i++)
        {
            configInt[i] = fut[i];
        }
        if(configInt[2] == 0)//判断机臂前后T(B)
        {
            tmpStr = "B," +tmpStr;
        }
        else
        {
            tmpStr = "T," + tmpStr;
        }
        if(configInt[1] == 0)//判断机臂上下Ｕ(D)
        {
            tmpStr = "D " +tmpStr;
        }
        else
        {
            tmpStr = "U " + tmpStr;
        }
        if(configInt[0] == 0)//判断手腕上下F(N)
        {
            tmpStr = "'N " +tmpStr;
        }
        else
        {
            tmpStr = "'F " + tmpStr;
        }

        configString = tmpStr.toStdString();
        return 1;
}

int CoordinateSolver::getFkPos(const JntArray& q_in, Frame& pos)
{ 

    int getFkPosReturn=-1;
    switch(robotType)
    {
    case ENUM_DESCART_WRACTEST_ROBOT:
    {
        return -1;//不支持
    }
    case ENUM_DESCART_ROBOT:
    {
        return getFkPos_descart(q_in, pos);
    }
    case ENUM_DESCART_XYZR_ROBOT:
    {
        return getFkPos_descart_XYZR(q_in, pos);
    }
        case ENUM_SCARA:
        {
            getFkPosReturn=fkSolverPos->JntToCart(q_in, pos);
        }
        break;
        case ENUM_4AXIS_PALLET:
        {
            getFkPosReturn=fkSolverPos->JntToCart(q_in, pos);
        }
        break;

        case ENUM_DESCART_WRAC_ROBOT:
    {
        getFkPosReturn=fkSolverPos->JntToCart(q_in, pos);
    }
        break;
        case ENUM_DESCART_WRIST_ROBOT:
    {
        getFkPosReturn=fkSolverPos->JntToCart(q_in, pos);

    }
    break;
        case ENUM_6AXIS_CHAIN:
        {
            JntArray tmpQIn=q_in;
            tmpQIn(1)=tmpQIn(1)+M_PI_2;
            getFkPosReturn=fkSolverPos->JntToCart(tmpQIn, pos);

        }
        break;
        case ENUM_7AXIS:
        {
            getFkPosReturn=fkSolverPos->JntToCart(q_in, pos);
        }
        break;
        case ENUM_DELTA_ROBOT_3Axis:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7065,robotId,robotType);
            getFkPosReturn=-1;
        }
        break;
        case ENUM_DELTA_ROBOT_4Axis:
        {
            getFkPosReturn=getFkPos_delta(q_in, pos);
//            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7065,robotId,robotType);
//            getFkPosReturn=-1;
        }
        break;
        case ENUM_UR_6AXIS:
        {
        if(0)
        {
            JntArray tmpQIn=q_in;
            tmpQIn(1)=tmpQIn(1)-M_PI_2;
            tmpQIn(3)=tmpQIn(3)-M_PI_2;

            getFkPosReturn=fkSolverPos->JntToCart(tmpQIn, pos);
            double roll,pitch,yaw;
            pos.M.GetRPY(roll,pitch,yaw);
            qDebug()<<"JntToCart GetRPY"<<roll<<pitch<<yaw;
            pos.M.GetEulerZYZ(roll,pitch,yaw);
            qDebug()<<"GetEulerZYZ"<<roll<<pitch<<yaw;
        }
        else
        {
            getFkPosReturn=getFkPos_ur(q_in, pos);
//            double roll,pitch,yaw;
//            pos.M.GetRPY(roll,pitch,yaw);
//            qDebug()<<"GetRPY"<<roll<<pitch<<yaw;
//            pos.M.GetEulerZYZ(roll,pitch,yaw);
//            qDebug()<<"GetEulerZYZ"<<roll<<pitch<<yaw;
        }



        }
        break;
    case ENUM_HEXPOD_6AXIS:
    {
        getFkPosReturn=getFkPos_hexpod(q_in, pos);

    }
    break;
        case ENUM_DELTA_ROBOT_4Axis_EIGHT_LEG:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7065,robotId,robotType);
            getFkPosReturn=-1;
        }
        break;

        default:
        {
            getFkPosReturn=fkSolverPos->JntToCart(q_in, pos);
        }
    }
    return getFkPosReturn;
}

int CoordinateSolver::getFkVel(const JntArrayVel& qvel_in, FrameVel& vel)
{
    return fkSolverVel->JntToCart(qvel_in, vel);
    int getFkVelReturn=-1;
    switch(robotType)
    {
        case ENUM_SCARA:
        {
            getFkVelReturn=fkSolverVel->JntToCart(qvel_in, vel);
        }
        break;
        case ENUM_4AXIS_PALLET:
        {
            getFkVelReturn=fkSolverVel->JntToCart(qvel_in, vel);
        }
        break;

        case ENUM_DESCART_WRIST_ROBOT:
        case ENUM_6AXIS_CHAIN:
        {
            getFkVelReturn=fkSolverVel->JntToCart(qvel_in, vel);

        }
        break;

        case ENUM_7AXIS:
        {
            getFkVelReturn=fkSolverVel->JntToCart(qvel_in, vel);
        }
        break;
        case ENUM_DELTA_ROBOT_3Axis:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7066,robotId,robotType);
            getFkVelReturn=-1;
        }
        break;
        case ENUM_DESCART_WRACTEST_ROBOT:
        case ENUM_DESCART_WRAC_ROBOT:
        case ENUM_HEXPOD_6AXIS:
        case ENUM_UR_6AXIS:
        case ENUM_DELTA_ROBOT_4Axis:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7066,robotId,robotType);
            getFkVelReturn=-1;
        }
        break;
        case ENUM_DELTA_ROBOT_4Axis_EIGHT_LEG:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7066,robotId,robotType);
            getFkVelReturn=-1;
        }
        break;

        default:
        {
            getFkVelReturn=fkSolverVel->JntToCart(qvel_in, vel);
        }
    }
    return getFkVelReturn;
}

int CoordinateSolver::getIkPos_NR_JL(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
//    joints = JntArray(robotChain->getNrOfJoints());
    return ikSolverPos_NR_JL->CartToJnt(q_init, p_in, joints);
}

int CoordinateSolver::getIkPos_NR(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
//    joints = JntArray(robotChain->getNrOfJoints());
    return ikSolverPos_NR->CartToJnt(q_init, p_in, joints);
}

int CoordinateSolver::getIkPos_LMA(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
//    joints = JntArray(robotChain->getNrOfJoints());
    return ikSolverPos_LMA->CartToJnt(q_init, p_in, joints);
}

int CoordinateSolver::getIkVel_pinv(const JntArray& q_in, const Twist& v_in, JntArray& jointsDot)
{
//    jointsDot = JntArray(robotChain->getNrOfJoints());
    return ikSolverVel_pinv->CartToJnt(q_in, v_in, jointsDot);
}

int CoordinateSolver::getIkVel_pinvGivens(const JntArray &q_in, const Twist &v_in, JntArray &jointsDot)
{
    return ikSolverVel_pinvGivens->CartToJnt(q_in, v_in, jointsDot);
}

int CoordinateSolver::getIdSolver_RNE(const JntArray &q, const JntArray &q_dot,
                                      const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
{
    return idSolver_RNE->CartToJnt(q, q_dot, q_dotdot, f_ext, torques);
}

int CoordinateSolver::getGravity(const JntArray &q, JntArray &gravity)
{
    return dynParam->JntToGravity(q, gravity);
}

int  CoordinateSolver::getIkPos_quick_fut(QVector<int> futConfig, const Frame& p_in, JntArray& joints
                                          , bool &isSinguratyOut)
{
    isSinguratyOut=false;
    switch(robotType)
    {
    case ENUM_DESCART_ROBOT:
    {
        return getIkPos_descart(p_in,  joints);
    }
    case ENUM_DESCART_XYZR_ROBOT:
    {
        return getIkPos_descart_XYZR_FUT(futConfig, p_in,  joints);
    }
    case ENUM_SCARA:
    {
        return getIkPos_SCARA_FUT(futConfig,  p_in,  joints);
    }
    case ENUM_DESCART_WRACTEST_ROBOT:
    case ENUM_4AXIS_PALLET:
    {

        addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7069,robotId);
        return -1;
    }
   case ENUM_6AXIS_CHAIN:
   {
       int ikReturn;
       ikReturn=getIkPos_SIX_FUT(futConfig,p_in,joints);
       return ikReturn;

    }
    case ENUM_DESCART_WRAC_ROBOT:
    {
        int ikReturn;
        ikReturn=getIkPos_decartwrac_FUT(futConfig,p_in,joints);
        return ikReturn;
    }
    case ENUM_DESCART_WRIST_ROBOT:
    {
        int ikReturn;
        ikReturn=getIkPos_decartwrist_FUT(futConfig,p_in,joints);
        return ikReturn;

     }
    case ENUM_7AXIS:
    {
        addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7069,robotId);
        return -1;
    }
    case ENUM_DELTA_ROBOT_4Axis:
    {
        int ikReturn;
        ikReturn=getIkPos_delta_FUT(futConfig,p_in,joints);
        return ikReturn;
    }
    case ENUM_UR_6AXIS:
    {
        int ikReturn;
        ikReturn=getIkPos_ur_FUT(futConfig,p_in,joints,isSinguratyOut);
        return ikReturn;
    }
    case ENUM_HEXPOD_6AXIS:
    {
        int ikReturn;
        ikReturn=getIkPos_hexpod(p_in,joints);
        return ikReturn;
    }


        default:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7069,robotId);
            return -1;
        }
    }

    return 1;
}



int  CoordinateSolver::getIkPos_quick(const JntArray& q_init, const Frame& p_in, JntArray& joints
                                      ,bool &isSinguratyOut)
{
    isSinguratyOut=false;
    switch(robotType)
    {
    case ENUM_DESCART_ROBOT:
    {
        return getIkPos_descart(p_in,  joints);
    }
    case ENUM_DESCART_XYZR_ROBOT:
    {
        return getIkPos_descart_XYZR(q_init,p_in,  joints);
    }
        case ENUM_SCARA:
        {
            int ikReturn;
           //ikReturn= getIkPos_NR( q_init,  p_in,  joints);  //J1900 CPU 28~80us, worst 3045us.
           //ikReturn=getIkPos_LMA( q_init,  p_in,  joints); // J1900 CPU 25~200us, worst 1790us.
            ikReturn=getIkPos_SCARA( q_init,  p_in,  joints);// J1900 CPU 3~100us, worst 3045us.
            if(1==ikReturn)
            {
                if(1!=checkIkSolverJointOffset(q_init,joints))
                {
                    qDebug()<<"p_in"<<p_in.p[0]<<p_in.p[1]<<p_in.p[2]<<" ikReturn="<<ikReturn;
                     addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7064,robotId);
    //                 getIkPos_SCARA( q_init,  p_in,  joints);
                     return -1;
                }
            }

            return ikReturn;
        }
        case ENUM_DESCART_WRACTEST_ROBOT:
        case ENUM_4AXIS_PALLET:
        {

            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7070,robotId);
            return -1;
        }
       case ENUM_6AXIS_CHAIN:
       {
           int ikReturn;
           //ikReturn=getIkPos_SIX( q_init,  p_in,  joints);  //J1900 CPU 18~69us, worst 200us.
          //ikReturn= getIkPos_NR( q_init,  p_in,  joints);  //J1900 CPU 28~80us, worst 3045us.
          //ikReturn=getIkPos_LMA( q_init,  p_in,  joints); // J1900 CPU 25~200us, worst 1790us.
           ikReturn=getIkPos_SIX( q_init,  p_in,  joints);// J1900 CPU 3~100us, worst 3045us.
           if(1==ikReturn)
           {
//               if(1!=checkIkSolverJointOffset(q_init,joints))
//               {
//                   qDebug()<<"p_in"<<p_in.p[0]<<p_in.p[1]<<p_in.p[2]<<" ikReturn="<<ikReturn;
//                    addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7064,robotId);
//   //                 getIkPos_SCARA( q_init,  p_in,  joints);
//                    return -1;
//               }
           }
           return ikReturn;
//           enum {
//           /// Converged but degraded solution (e.g. WDLS with psuedo-inverse singular)
//               E_DEGRADED         = +1,
//           //! No error
//               E_NOERROR          =  0,
//           //! Failed to converge
//               E_NO_CONVERGE      = -1,
//           //! Undefined value (e.g. computed a NAN, or tan(90 degrees) )
//               E_UNDEFINED        = -2,
//           //! Input size does not match internal state
//               E_SIZE_MISMATCH = -4,
//           //! Maximum number of iterations exceeded
//               E_MAX_ITERATIONS_EXCEEDED = -5,
//           //! Requested index out of range
//               E_OUT_OF_RANGE = -6,
//           //! Not yet implemented
//               E_NOT_IMPLEMENTED = -7,
//           //! Internal svd calculation failed
//               E_SVD_FAILED = -8
//           };
        }
    case ENUM_DESCART_WRAC_ROBOT:
    {
        int ikReturn;
        ikReturn=getIkPos_decartwrac( q_init,  p_in,  joints);// J1900 CPU 3~100us, worst 3045us.
        if(1==ikReturn)
        {
            if(1!=checkIkSolverJointOffset(q_init,joints))
            {
                qDebug()<<"p_in"<<p_in.p[0]<<p_in.p[1]<<p_in.p[2]<<" ikReturn="<<ikReturn;
                 addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7064,robotId);
                 return -1;
            }
        }
        return ikReturn;
    }
    case ENUM_DESCART_WRIST_ROBOT:
    {
        int ikReturn;
        //ikReturn=getIkPos_SIX( q_init,  p_in,  joints);  //J1900 CPU 18~69us, worst 200us.
       //ikReturn= getIkPos_NR( q_init,  p_in,  joints);  //J1900 CPU 28~80us, worst 3045us.
       //ikReturn=getIkPos_LMA( q_init,  p_in,  joints); // J1900 CPU 25~200us, worst 1790us.
        ikReturn=getIkPos_decartwrist( q_init,  p_in,  joints);// J1900 CPU 3~100us, worst 3045us.
        if(1==ikReturn)
        {
            if(1!=checkIkSolverJointOffset(q_init,joints))
            {
                qDebug()<<"p_in"<<p_in.p[0]<<p_in.p[1]<<p_in.p[2]<<" ikReturn="<<ikReturn;
                 addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7064,robotId);
//                 getIkPos_SCARA( q_init,  p_in,  joints);
                 return -1;
            }
        }
        return ikReturn;

     }
    case ENUM_7AXIS:
    {
        addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7070,robotId);
        return -1;
    }
    case ENUM_DELTA_ROBOT_4Axis:
    {
         return getIkPos_delta(q_init,  p_in,  joints);

    }
    case ENUM_UR_6AXIS:
    {
         return getIkPos_ur(q_init,  p_in,  joints,isSinguratyOut);

    }
    case ENUM_HEXPOD_6AXIS:
    {
         return getIkPos_hexpod( p_in,  joints);

    }

        default:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7070,robotId);
            return -1;
        }
    }

    return 1;
}

int  CoordinateSolver::getIkPos_iteration(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
    switch(robotType)
    {
        case ENUM_SCARA:
        {
            int ikReturn;
           //ikReturn= getIkPos_NR( q_init,  p_in,  joints);  //J1900 CPU 28~80us, worst 3045us.
           //ikReturn=getIkPos_LMA( q_init,  p_in,  joints); // J1900 CPU 25~200us, worst 1790us.
            ikReturn=getIkPos_NR_JL( q_init,  p_in,  joints);// J1900 CPU 3~100us, worst 3045us.
            if(1!=checkIkSolverJointOffset(q_init,joints))
            {
                 addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7064,robotId);
                 return -1;
            }
            return ikReturn;
        }
        case ENUM_4AXIS_PALLET:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7070,robotId);
            return -1;
        }
       case ENUM_6AXIS_CHAIN:
       {
           int ikReturn;
           //ikReturn=getIkPos_SIX( q_init,  p_in,  joints);  //J1900 CPU 18~69us, worst 200us.
          //ikReturn= getIkPos_NR( q_init,  p_in,  joints);  //J1900 CPU 28~80us, worst 3045us.
          //ikReturn=getIkPos_LMA( q_init,  p_in,  joints); // J1900 CPU 25~200us, worst 1790us.
           ikReturn=getIkPos_NR_JL( q_init,  p_in,  joints);// J1900 CPU 3~100us, worst 3045us.
           if(1!=checkIkSolverJointOffset(q_init,joints))
           {
                addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7064,robotId);
                return -1;
           }
           return ikReturn;
//           enum {
//           /// Converged but degraded solution (e.g. WDLS with psuedo-inverse singular)
//               E_DEGRADED         = +1,
//           //! No error
//               E_NOERROR          =  0,
//           //! Failed to converge
//               E_NO_CONVERGE      = -1,
//           //! Undefined value (e.g. computed a NAN, or tan(90 degrees) )
//               E_UNDEFINED        = -2,
//           //! Input size does not match internal state
//               E_SIZE_MISMATCH = -4,
//           //! Maximum number of iterations exceeded
//               E_MAX_ITERATIONS_EXCEEDED = -5,
//           //! Requested index out of range
//               E_OUT_OF_RANGE = -6,
//           //! Not yet implemented
//               E_NOT_IMPLEMENTED = -7,
//           //! Internal svd calculation failed
//               E_SVD_FAILED = -8
//           };


        }
    case ENUM_7AXIS:
    {
        int ikReturn;
       //ikReturn= getIkPos_NR( q_init,  p_in,  joints);  //J1900 CPU 28~80us, worst 3045us.
       ikReturn=getIkPos_LMA( q_init,  p_in,  joints); // J1900 CPU 25~200us, worst 1790us.
        //ikReturn=getIkPos_NR_JL( q_init,  p_in,  joints);// J1900 CPU 3~100us, worst 3045us.
//        if(1!=checkIkSolverJointOffset(q_init,joints))
//        {
//             addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7064,robotId);
//             return -1;
//        }
        return ikReturn;
    }
        default:
        {
            addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7070,robotId);
            return -1;
        }
    }

    return 1;
}

#ifndef BETA
int CoordinateSolver::getIkPos_SCARA(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
    //多组解临时变量
    double ikSolution[2][4];
    //目标矩阵格式转换，Frame -> Matrix4d
//    Matrix4d pose;
//    //x y z
//    pose(0,3)=p_in.p.data[0];
//    pose(1,3)=p_in.p.data[1];
//    pose(2,3)=p_in.p.data[2];
//    //旋转矩阵
//    pose(0,0)=p_in.M.data[0];
//    pose(0,1)=p_in.M.data[1];
//    pose(0,2)=p_in.M.data[2];
//    pose(1,0)=p_in.M.data[3];
//    pose(1,1)=p_in.M.data[4];
//    pose(1,2)=p_in.M.data[5];
//    pose(2,0)=p_in.M.data[6];
//    pose(2,1)=p_in.M.data[7];
//    pose(2,2)=p_in.M.data[8];

    //原来的DH[i].q=0 alpha=1 a=2 d=3 用( *robotJoints)[i].dhParameter[0]替换
    //几何解法的中间参数变量
    double x = p_in.p.data[0];//x坐标
    double y = p_in.p.data[1];//y坐标
    double r = sqrt(x*x + y*y);
    double a = ( *robotJoints)[0].dhParameter[0];//大臂长
    double b = ( *robotJoints)[1].dhParameter[0];//小臂长
    double angleRotation = atan2(y, x);
    double rad2 = p_in.p.data[2] - (*robotJoints)[0].dhParameter[2] - (*robotJoints)[1].dhParameter[2] - (*robotJoints)[3].dhParameter[2];//三轴实际位移量
    // rad2=(pose(2,3)-DH[0].d-DH[1].d-DH[3].d)/scaraSliderFactor*2*M_PI;

//    if(IF_DEBUG)
//    {
////        qDebug()<<"CoordinateSolver::getIkPos_SCARA====q_init:";
////        for(int i=0; i<4; i++)
////        {
////            qDebug()<< i <<q_init(i);
////        }
//        std::cout<<"p_in"<<p_in<<std::endl;
//    }

    if ( (a + b < r)||
         (fabs(b - a) >= r))
    {
        if(IF_DEBUG)
        {
            qDebug() << "CoordinateSolver::getIkPos_SCARA====位置超过工作空间，不可达"<<a<<b<<r;
        }
        return -1;//位置超过工作空间，不可达。
    }
    else if(a+b==r)
    {
        joints(0) = angleRotation;
        joints(1) = 0.0;
        joints(2) = rad2;
        joints(3) = acos( p_in.M.data[0] );
        return 0;
    }
    else
    {
        double angle1 = acos((a*a + r*r - b*b)/(2*a*r));//余弦定理
        double angle2 = acos((a*a + b*b - r*r)/(2*a*b));
        ikSolution[0][2]=rad2;
        ikSolution[1][2]=rad2;
        double y2;
        double p;
        p=atan2(-p_in.M.data[6],sqrt(p_in.M.data[0]*p_in.M.data[0]+p_in.M.data[2]*p_in.M.data[2]));
        y2=atan2(p_in.M.data[3]/cos(p),p_in.M.data[0]/cos(p));

        //左手解
        if ( angleRotation + angle1 > M_PI)
        {
            ikSolution[0][0]=angleRotation + angle1 - 2.0*M_PI;
        }else
        {
            ikSolution[0][0]=angleRotation + angle1;
        }
        ikSolution[0][1] = angle2 - M_PI;
        ikSolution[0][3] = y2 - ikSolution[0][0] - ikSolution[0][1];

        //右手解
        if (angleRotation - angle1 > -M_PI)
        {
            ikSolution[1][0]=angleRotation - angle1;
        }
        else
        {
            ikSolution[1][0]=angleRotation - angle1 + 2.0*M_PI;
        }
        ikSolution[1][1] = M_PI - angle2;
        ikSolution[1][3] = y2 - ikSolution[1][0] - ikSolution[1][1];
    }

    //旋转轴角度转换到－180～180
    for(int i=0; i<2; i++)
    {
          if(ikSolution[i][3] > M_PI)
          {
              ikSolution[i][3]=ikSolution[i][3] - 2*M_PI*ceil(ikSolution[i][3]/2.0/M_PI);
          }
          else if(ikSolution[i][3] <= -M_PI)
          {
              ikSolution[i][3]=ikSolution[i][3] - 2*M_PI*floor(ikSolution[i][3]/2.0/M_PI);
          }
          if((ikSolution[i][3] > M_PI)||(ikSolution[i][3] <= -M_PI))
          {
              qDebug()<<"CoordinateSolver::getIkPos_SCARA===="<<ikSolution[i][3];
          }
//        ikSolution[i][3]=ikSolution[i][3] - 2.0*M_PI*round(ikSolution[i][3]/2.0/M_PI);
    }

    //选择最优解，左手，右手
    if(q_init(1)>0)//右手解
    {
        for(int k=0;k<4;k++)
        {
            joints(k)=ikSolution[1][k];
        }
    }
    else//左手解
    {
        for(int k=0;k<4;k++)
        {
            joints(k)=ikSolution[0][k];
        }
    }

    return 0;
}

#else

int CoordinateSolver::getIkPos_SCARA_FUT(QVector<int> futConfig, const Frame& p_in, JntArray& joints)
{
    JntArray q_init(4);
    q_init(0)=futConfig[0]*M_PI*2;
    q_init(1)=futConfig[1]*M_PI*2;
    q_init(2)=futConfig[2]*M_PI*2;
    q_init(3)=futConfig[3]*M_PI*2;
    int tmpReturn= getIkPos_SCARA(q_init,p_in,joints);

    return tmpReturn;
}

int CoordinateSolver::getIkPos_descart(const Frame &p_in, JntArray &joints)
{
    //没考虑dh偏移
    JntArray q_init(3);
    q_init(0)=p_in.p[0];
    q_init(1)=p_in.p[1];
    q_init(2)=p_in.p[2];
    joints=q_init;
    return 1;
}

int CoordinateSolver::getFkPos_descart(const JntArray &q_in, Frame &pos)
{
    if(3!=q_in.rows())
    {
        qDebug()<<"error,,,getFkPos_descart 3!=q_in.rows()";
    }
    pos.p[0]=q_in(0);
    pos.p[1]=q_in(1);
    pos.p[2]=q_in(2);
    pos.M.Identity()=pos.M.Identity();
    return 1;
}

int CoordinateSolver::getIkPos_descart_XYZR(const JntArray &q_initIn,const Frame &p_in, JntArray &joints)
{
    JntArray q_init=q_initIn;
    q_init(0)=p_in.p[0];
    q_init(1)=p_in.p[1];
    q_init(2)=p_in.p[2];

    double tmpR,tmpP,tmpY;
    p_in.M.GetRPY(tmpR,tmpP,tmpY);
    getBestPositionFromAngle_Rad(tmpY,q_init(3),tmpY);
//    q_init(3)=tmpY/M_PI*180.0;
    q_init(3)=tmpY;
    joints=q_init;
    return 1;
}

int CoordinateSolver::getFkPos_descart_XYZR(const JntArray &q_in, Frame &pos)
{
    if(4!=q_in.rows())
    {
        qDebug()<<"error,,,getFkPos_descart_XYZR 4!=q_in.rows()";
    }
    pos.p[0]=q_in(0);
    pos.p[1]=q_in(1);
    pos.p[2]=q_in(2);
//    pos.M=pos.M.RPY(0,0,q_in(3)/M_PI*180.0);
    pos.M=pos.M.RPY(0,0,q_in(3));
    return 1;
}

int CoordinateSolver::getIkPos_descart_XYZR_FUT(QVector<int> futConfig, const Frame &p_in, JntArray &joints)
{
    JntArray q_init(4);
    q_init(0)=0;
    q_init(1)=0;
    q_init(2)=0;
    q_init(3)=futConfig[0]/180.0*M_PI;
    int tmpReturn= getIkPos_descart_XYZR(q_init,p_in,joints);

    return tmpReturn;
}


int CoordinateSolver::getIkPos_SCARA(const JntArray &q_init, const Frame& p_in, JntArray& joints)
{


    //原来的DH[i].q=0 alpha=1 a=2 d=3 用( *robotJoints)[i].dhParameter[0]替换
    //几何解法的中间参数变量
    double eps = 1e-10;
    double x = p_in.p.data[0];//x坐标
    double y = p_in.p.data[1];//y坐标
    double r = sqrt(x*x + y*y);
    double a = ( *robotJoints)[0].dhParameter[0];//大臂长
    double b = ( *robotJoints)[1].dhParameter[0];//小臂长
    double angleRotation = atan2(y, x);
    double y2 = atan2(-p_in.M.data[1], p_in.M.data[0]);//r角
    joints(2)  = (p_in.p.data[2] - (*robotJoints)[0].dhParameter[2] - (*robotJoints)[1].dhParameter[2] - (*robotJoints)[3].dhParameter[2])
            /cos((*robotJoints)[2].dhParameter[2]);//三轴实际位移量

    if ( (a + b < r + eps)||
         (fabs(b - a) >= r))
    {
//        if(IF_DEBUG)
//        {
//            qDebug() << "CoordinateSolver::getIkPos_SCARA====位置超过工作空间，不可达"<<a<<b<<r;
//        }

        return -1;//位置超过工作空间，不可达。
    }
    else if(eps >= fabs(a+b-r))
    {
        joints(0) = angleRotation;
        joints(1) = 0.0;
//        joints(3) = acos( p_in.M.data[0] );//acos 0~pi   atan2(-p_in.M.data[5], p_in.M.data[8])//范围-pi~pi
        joints(3) = y2 - angleRotation;
    }
    else
    {
        double angle1 = acos((a*a + r*r - b*b)/(2*a*r));//余弦定理0~pi
        double angle2 = acos((a*a + b*b - r*r)/(2*a*b));//余弦定理0~pi
        if( q_init(1)<0 )//左手
        {
            joints(1) = angle2 - M_PI;
            joints(0) = angleRotation + angle1;
            if(joints(0) > M_PI)
            {
                joints(0) = joints(0) - 2.0*M_PI;
            }
        }
        else//右手
        {
            joints(1) = M_PI - angle2;
            joints(0) = angleRotation - angle1;
            if(joints(0) < -M_PI)
            {
                joints(0) = joints(0) + 2.0*M_PI;
            }
        }
        joints(3) = y2 - joints(0) - joints(1);
    }
//    if(joints(3) > M_PI)
//    {
//        joints(3) = joints(3) - 2*M_PI*ceil(joints(3)/2.0/M_PI);
//    }
//    else if(joints(3) <= -M_PI)
//    {
//        joints(3) = joints(3) - 2*M_PI*floor(joints(3)/2.0/M_PI);
//    }
//    if((joints(3) > M_PI)||(joints(3) <= -M_PI))
//    {
//        qDebug()<<"CoordinateSolver::getIkPos_SCARA====ERROR"<<joints(3);
//    }

//    //支持多圈
//    joints(0)+=2*M_PI*getTurnsFromRad(q_init(0));
//    joints(1)+=2*M_PI*getTurnsFromRad(q_init(1));
//    joints(3)+=2*M_PI*getTurnsFromRad(q_init(3));
    //支持多圈
    getBestPositionFromAngle_Rad(joints(0),q_init(0),joints(0));
    getBestPositionFromAngle_Rad(joints(1),q_init(1),joints(1));
    getBestPositionFromAngle_Rad(joints(3),q_init(3),joints(3));

    return 0;
}

int CoordinateSolver::getIkPos_SIX_FUT(QVector<int> futConfig, const Frame &p_in, JntArray &joints)
{
    int fut[6];
    for(int i=0;i<6;i++)
    {
        fut[i]=futConfig[i];
    }
    //初始化中间变量
    double tmpPoseMatrix[4][4];
    double tmpJointSolution[6];

    tmpPoseMatrix[0][0]=p_in.M.data[0];
    tmpPoseMatrix[0][1]=p_in.M.data[1];
    tmpPoseMatrix[0][2]=p_in.M.data[2];
    tmpPoseMatrix[1][0]=p_in.M.data[3];
    tmpPoseMatrix[1][1]=p_in.M.data[4];
    tmpPoseMatrix[1][2]=p_in.M.data[5];
    tmpPoseMatrix[2][0]=p_in.M.data[6];
    tmpPoseMatrix[2][1]=p_in.M.data[7];
    tmpPoseMatrix[2][2]=p_in.M.data[8];

    tmpPoseMatrix[0][3]=p_in.p[0];
    tmpPoseMatrix[1][3]=p_in.p[1];
    tmpPoseMatrix[2][3]=p_in.p[2];
    tmpPoseMatrix[3][3]=1;
    tmpPoseMatrix[3][0]=0;
    tmpPoseMatrix[3][1]=0;
    tmpPoseMatrix[3][2]=0;

    //求解
    SixJointRobot sixJointRobotSolver;
    sixJointRobotSolver.initialForIk(robotDhParameter);
    int isIK = sixJointRobotSolver.getIkSolutionFUT(tmpPoseMatrix,fut,tmpJointSolution);
    if(1!=isIK)
    {
        qDebug()<<"get ik failed!getIkSolutionFUT";
        return isIK;
    }

    //求解值转换
    for(int i=0;i<6;i++)
    {
            joints(i)=tmpJointSolution[i];
    }

    return 0;
}

int CoordinateSolver::getIkPos_decartwrist(const JntArray &q_init, const Frame &p_in, JntArray &joints)
{
    //初始化中间变量
    double tmpPoseMatrix[4][4];
    double tmpBeforeJoint[6];
    double tmpJointSolution[6];

    tmpPoseMatrix[0][0]=p_in.M.data[0];
    tmpPoseMatrix[0][1]=p_in.M.data[1];
    tmpPoseMatrix[0][2]=p_in.M.data[2];
    tmpPoseMatrix[1][0]=p_in.M.data[3];
    tmpPoseMatrix[1][1]=p_in.M.data[4];
    tmpPoseMatrix[1][2]=p_in.M.data[5];
    tmpPoseMatrix[2][0]=p_in.M.data[6];
    tmpPoseMatrix[2][1]=p_in.M.data[7];
    tmpPoseMatrix[2][2]=p_in.M.data[8];

    tmpPoseMatrix[0][3]=p_in.p[0];
    tmpPoseMatrix[1][3]=p_in.p[1];
    tmpPoseMatrix[2][3]=p_in.p[2];
    tmpPoseMatrix[3][3]=1;
    tmpPoseMatrix[3][0]=0;
    tmpPoseMatrix[3][1]=0;
    tmpPoseMatrix[3][2]=0;


    for(int j=0;j<6;j++)
    {
       tmpBeforeJoint[j]=q_init(j);
    }

    //求解
    DecartWrist sixJointRobotSolver;
    sixJointRobotSolver.initialForIk(robotDhParameter);
    int isIK = sixJointRobotSolver.getIkSolution(tmpPoseMatrix,tmpBeforeJoint,tmpJointSolution);
    if(1!=isIK)
    {
        qDebug()<<"DecartWrist get ik failed!";
        return isIK;
    }

    //求解值转换
    for(int i=0;i<6;i++)
    {
            joints(i)=tmpJointSolution[i];
    }

    //debug　将q_init，p_in，joints写入文件, 查看逆解数据
//    dataRecord->addItem();
//    for (int i = 0; i < 6; i++)
//    {
//        dataRecord->writeRecord(q_init(i));
//    }
//    for (int i = 0; i < 3; i++)
//    {
//        dataRecord->writeRecord(p_in.p.data[i]);
//    }
//    for (int i = 0; i < 6; i++)
//    {
//        dataRecord->writeRecord(joints(i));
//    }

    return 1;
}

int CoordinateSolver::getIkPos_decartwrac(const JntArray &q_init, const Frame &p_in, JntArray &joints)
{
    //初始化中间变量
    double tmpPoseMatrix[4][4];
    double tmpBeforeJoint[6];
    double tmpJointSolution[6];

    tmpPoseMatrix[0][0]=p_in.M.data[0];
    tmpPoseMatrix[0][1]=p_in.M.data[1];
    tmpPoseMatrix[0][2]=p_in.M.data[2];
    tmpPoseMatrix[1][0]=p_in.M.data[3];
    tmpPoseMatrix[1][1]=p_in.M.data[4];
    tmpPoseMatrix[1][2]=p_in.M.data[5];
    tmpPoseMatrix[2][0]=p_in.M.data[6];
    tmpPoseMatrix[2][1]=p_in.M.data[7];
    tmpPoseMatrix[2][2]=p_in.M.data[8];

    tmpPoseMatrix[0][3]=p_in.p[0];
    tmpPoseMatrix[1][3]=p_in.p[1];
    tmpPoseMatrix[2][3]=p_in.p[2];
    tmpPoseMatrix[3][3]=1;
    tmpPoseMatrix[3][0]=0;
    tmpPoseMatrix[3][1]=0;
    tmpPoseMatrix[3][2]=0;


    for(int j=0;j<5;j++)
    {
       tmpBeforeJoint[j]=q_init(j);
    }

    //求解
    DecartWrist sixJointRobotSolver;
    sixJointRobotSolver.initialForIk(robotDhParameter);
    int isIK = sixJointRobotSolver.getIkSolution_5axisAc(tmpPoseMatrix,tmpBeforeJoint,tmpJointSolution);
    if(1!=isIK)
    {
        qDebug()<<"DecartWrist get ik failed!";
        return isIK;
    }

    //求解值转换
    for(int i=0;i<5;i++)
    {
            joints(i)=tmpJointSolution[i];
    }



    return 1;
}

int CoordinateSolver::getIkPos_decartwrist_FUT(QVector<int> futConfig, const Frame &p_in, JntArray &joints)
{
    int fut[6];
    for(int i=0;i<futConfig.size();i++)
    {
        fut[i]=futConfig[i];
    }
    //初始化中间变量
    double tmpPoseMatrix[4][4];
    double tmpJointSolution[6];

    tmpPoseMatrix[0][0]=p_in.M.data[0];
    tmpPoseMatrix[0][1]=p_in.M.data[1];
    tmpPoseMatrix[0][2]=p_in.M.data[2];
    tmpPoseMatrix[1][0]=p_in.M.data[3];
    tmpPoseMatrix[1][1]=p_in.M.data[4];
    tmpPoseMatrix[1][2]=p_in.M.data[5];
    tmpPoseMatrix[2][0]=p_in.M.data[6];
    tmpPoseMatrix[2][1]=p_in.M.data[7];
    tmpPoseMatrix[2][2]=p_in.M.data[8];

    tmpPoseMatrix[0][3]=p_in.p[0];
    tmpPoseMatrix[1][3]=p_in.p[1];
    tmpPoseMatrix[2][3]=p_in.p[2];
    tmpPoseMatrix[3][3]=1;
    tmpPoseMatrix[3][0]=0;
    tmpPoseMatrix[3][1]=0;
    tmpPoseMatrix[3][2]=0;

    //求解
    DecartWrist sixJointRobotSolver;
    sixJointRobotSolver.initialForIk(robotDhParameter);
    int isIK = sixJointRobotSolver.getIkSolutionFUT(tmpPoseMatrix,fut[0],fut[1],fut[2],tmpJointSolution);
    if(1!=isIK)
    {
        qDebug()<<"DecartWrist getIkSolutionFUT failed!";
        return isIK;
    }

    //求解值转换
    for(int i=0;i<6;i++)
    {
            joints(i)=tmpJointSolution[i];
    }

    return 1;
}

int CoordinateSolver::getIkPos_decartwrac_FUT(QVector<int> futConfig, const Frame &p_in, JntArray &joints)
{
    int fut[6];
    for(int i=0;i<futConfig.size();i++)
    {
        fut[i]=futConfig[i];
    }
    //初始化中间变量
    double tmpPoseMatrix[4][4];
    double tmpJointSolution[6];

    tmpPoseMatrix[0][0]=p_in.M.data[0];
    tmpPoseMatrix[0][1]=p_in.M.data[1];
    tmpPoseMatrix[0][2]=p_in.M.data[2];
    tmpPoseMatrix[1][0]=p_in.M.data[3];
    tmpPoseMatrix[1][1]=p_in.M.data[4];
    tmpPoseMatrix[1][2]=p_in.M.data[5];
    tmpPoseMatrix[2][0]=p_in.M.data[6];
    tmpPoseMatrix[2][1]=p_in.M.data[7];
    tmpPoseMatrix[2][2]=p_in.M.data[8];

    tmpPoseMatrix[0][3]=p_in.p[0];
    tmpPoseMatrix[1][3]=p_in.p[1];
    tmpPoseMatrix[2][3]=p_in.p[2];
    tmpPoseMatrix[3][3]=1;
    tmpPoseMatrix[3][0]=0;
    tmpPoseMatrix[3][1]=0;
    tmpPoseMatrix[3][2]=0;

    //求解
    DecartWrist sixJointRobotSolver;
    sixJointRobotSolver.initialForIk(robotDhParameter);
    int isIK = sixJointRobotSolver.getIkSolutionFUT_5axisAc(tmpPoseMatrix,fut[0],fut[1],fut[2],tmpJointSolution);
    if(1!=isIK)
    {
        qDebug()<<"DecartWrist getIkSolutionFUT_5axisAc failed!";
        return isIK;
    }

    //求解值转换
    for(int i=0;i<5;i++)
    {
            joints(i)=tmpJointSolution[i];
    }

    return 0;
}

int CoordinateSolver::getIkPos_delta(const JntArray &q_init, const Frame &p_in, JntArray &joints)
{
//    DeltaKinematics<double> test_robot(deltaDhParameter);
//    DeltaKinematics<double>::DeltaVector my_vector = {p_in.p[0],p_in.p[1],p_in.p[2],0,0,0};
//        my_vector.Print();
//        if(0!=deltaRobotKinetic->CalculateIpk_bias(&my_vector, 1))
//        {
//            qDebug()<<"error,0!=test_robot.CalculateIpk(&my_vector, 1)";
//            return -1;
//        }
    double j1Out,j2Out,j3Out;
    if(1!=deltaRobotKinetic->inverse_bias(p_in.p[0],p_in.p[1],p_in.p[2],j1Out,j2Out,j3Out))
    {
        qDebug()<<"error,1!=test_robot.inverse_bias()  p_in"<<p_in.p[0]<<p_in.p[1]<<p_in.p[2];
        return -1;
    }
        double v3,v4,v5;
        extractEulerMatrixAngle(p_in.M ,v3, v4, v5);
        joints.resize(4);
//        joints.data[0]=my_vector.phi1/180.0*M_PI;
//        joints.data[1]=my_vector.phi2/180.0*M_PI;
//        joints.data[2]=my_vector.phi3/180.0*M_PI;
        joints.data[0]=j1Out/180.0*M_PI;
        joints.data[1]=j2Out/180.0*M_PI;
        joints.data[2]=j3Out/180.0*M_PI;
        joints.data[3]=v5;
        //支持多圈
        getBestPositionFromAngle_Rad(joints(3),q_init(3),joints(3));
        return 1;

}

int CoordinateSolver::getIkPos_delta_FUT(QVector<int> futConfig, const Frame &p_in, JntArray &joints)
{
    JntArray q_init(4);
    q_init(0)=0;
    q_init(1)=0;
    q_init(2)=0;
    q_init(3)=futConfig[0]*M_PI*2;
    int tmpReturn= getIkPos_delta(q_init,p_in,joints);

    return tmpReturn;
}

int CoordinateSolver::getFkPos_delta(const JntArray &q_in, Frame &p_out)
{
    if(3>q_in.rows())
    {
        qDebug()<<"error,getFkPos_delta 3>q_in.rows()";
        return -1;
    }
//    DeltaKinematics<double> test_robot(deltaDhParameter);
//    DeltaKinematics<double>::DeltaVector my_vector = {0,0,0,q_in.data[0]/M_PI*180.0,q_in.data[1]/M_PI*180.0,q_in.data[2]/M_PI*180.0};
//        my_vector.Print();
//    if(0!=deltaRobotKinetic->CalculateFpk_bias(&my_vector, 1))
//    {
//        qDebug()<<"error,0!=deltaRobotKinetic->CalculateFpk_bias(&my_vector, 1)";
//        return -1;
//    }
    double xOut,yOut,zOut;
    if(1!=deltaRobotKinetic->forward_bias(q_in.data[0]/M_PI*180.0,q_in.data[1]/M_PI*180.0,q_in.data[2]/M_PI*180.0,xOut,yOut,zOut))
    {
        qDebug()<<"error,1!=deltaRobotKinetic->forward_bias()";
        return -1;
    }

    p_out.p[0]=xOut;
    p_out.p[1]=yOut;
    p_out.p[2]=zOut;
    p_out.M=createEulerMatrix(0,0,q_in.data[3]);
    return 1;

}

int CoordinateSolver::getIkPos_ur(const  JntArray &q_init, const Frame &p_in, JntArray &joints
                                  ,bool &isSinguratyOut)
{
    isSinguratyOut=false;
    if(6>q_init.rows())
    {
        qDebug()<<"error,getIkPos_ur 6>q_in.rows()";
        return -1;
    }
//    Frame tmpFrame;
//    getFkPos_ur(q_init, tmpFrame);

    //test
    JntArray q_initNew=q_init;
    q_initNew(1)-=M_PI_2;
    q_initNew(3)-=M_PI_2;

    double qOut[48];
    double mat4_4[16];

//    tIn[3]=p_in.p[0];
//    tIn[7]=p_in.p[1];
//    tIn[11]=p_in.p[2];
//    tIn[0]=p_in.M.data[0];
//    tIn[1]=p_in.M.data[1];
//    tIn[2]=p_in.M.data[2];
//    tIn[4]=p_in.M.data[3];
//    tIn[5]=p_in.M.data[4];
//    tIn[6]=p_in.M.data[5];
//    tIn[8]=p_in.M.data[6];
//    tIn[9]=p_in.M.data[7];
//    tIn[10]=p_in.M.data[8];


    for(int i=0; i< 3;++i)
    {
        mat4_4[i*4+0] = p_in.M.data[i*3+0];
        mat4_4[i*4+1] = p_in.M.data[i*3+1];
        mat4_4[i*4+2] = p_in.M.data[i*3+2];
        mat4_4[i*4+3] = p_in.p[i];
    }
    mat4_4[3*4+0] = 0;
    mat4_4[3*4+1] = 0;
    mat4_4[3*4+2] = 0;
    mat4_4[3*4+3] = 1;


    int solveNum=urKinematic->inverse(mat4_4,qOut,q_initNew.data[5]);
    if(0==solveNum)
    {
        double tmpR,tmpP,tmpY;
        p_in.M.GetRPY(tmpR,tmpP,tmpY);
        qDebug()<<"error,urKinematic->inverse 无解  p_in"<<p_in.p[0]<<p_in.p[1]<<p_in.p[2]
               <<"rpy"<<tmpR<<tmpP<<tmpY;
        return -1;
    }

    JntArray tmpJoint[solveNum];
    double biasSum[solveNum];
    for(int i=0;i<solveNum;i++)
    {
        tmpJoint[i].resize(6);
        //支持多圈
        biasSum[i]=0;
        for(int j=0;j<6;j++)
        {
            qOut[j+i*6]=transformToPi(qOut[j+i*6]);
            getBestPositionFromAngle_Rad(qOut[j+i*6],q_initNew(j),tmpJoint[i](j));
            biasSum[i]+=fabs(q_initNew(j)-tmpJoint[i](j));
        }
    }
    //smallest
    int index=0;
    double tmpSmall=biasSum[0];
    for(int i=1;i<solveNum;i++)
    {
        if(biasSum[i]<tmpSmall)
        {
            tmpSmall=biasSum[i];
            index=i;
        }

    }

    if(0)
    {
        qDebug()<<"---------------------------------solveNum"<<solveNum<<"index"<<index
               <<"q_initNew"<<q_initNew(0)<<q_initNew(1)<<q_initNew(2)<<q_initNew(3)<<q_initNew(4)<<q_initNew(5);

        for(int i=0;i<solveNum;i++)
        {
            qDebug()<<i <<" qOut"<<qOut[0+i*6]<<qOut[1+i*6]
                   <<qOut[2+i*6]<<qOut[3+i*6]<<qOut[4+i*6]<<qOut[5+i*6];
            qDebug()<<i <<" biasSum"<<biasSum[i]<<"tmpJoint"<<tmpJoint[i](0)<<tmpJoint[i](1)
                      <<tmpJoint[i](2)<<tmpJoint[i](3)<<tmpJoint[i](4)<<tmpJoint[i](5)
                      <<tmpJoint[i](6);

        }
    }

    joints=tmpJoint[index];

    joints(1)+=M_PI_2;
    joints(3)+=M_PI_2;

    isSinguratyOut=urKinematic->getSinguratyResult(mat4_4,qOut[2+index*6],qOut[4+index*6]);


    return 1;
}

int CoordinateSolver::getIkPos_ur_FUT(QVector<int> futConfig, const Frame &p_in, JntArray &joints,
                                      bool &isSinguratyOut)
{
    JntArray q_init(6);
    for(int i=0;i<6;i++)
    {
        q_init(i)=M_PI*futConfig[i]/180.0;
    }

    int tmpReturn= getIkPos_ur(q_init,p_in,joints,isSinguratyOut);

    return tmpReturn;
}

int CoordinateSolver::getFkPos_ur(const JntArray &q_in, Frame &p_out)
{
    if(6>q_in.rows())
    {
        qDebug()<<"error,getFkPos_ur 6>q_in.rows()";
        return -1;
    }

    double tOut[16];
    double qIn[6];
    for(int i=0;i<6;i++)
    {
        qIn[i]=q_in.data[i];
    }
    if(1!=urKinematic->forward_bias(qIn,tOut))
    {
        qDebug()<<"error,1!=urKinematic->forward()";
        return -1;
    }

//    p_out.p[0]=tOut[3];
//    p_out.p[1]=tOut[7];
//    p_out.p[2]=tOut[11];
//    p_out.M.data[0]=tOut[0];
//    p_out.M.data[1]=tOut[1];
//    p_out.M.data[2]=tOut[2];
//    p_out.M.data[3]=tOut[4];
//    p_out.M.data[4]=tOut[5];
//    p_out.M.data[5]=tOut[6];
//    p_out.M.data[6]=tOut[8];
//    p_out.M.data[7]=tOut[9];
//    p_out.M.data[8]=tOut[10];

    for(int i=0; i< 3;++i)
    {
          p_out.M.data[i*3+0] = tOut[i*4+0];
          p_out.M.data[i*3+1] = tOut[i*4+1];
          p_out.M.data[i*3+2] = tOut[i*4+2];
          p_out.p[i] = tOut[i*4+3];
     }

    return 1;
}

int CoordinateSolver::getIkPos_hexpod(const Frame &p_in, JntArray &jointsOut)
{
    EmcPose  pos;

    pos.tran.x=p_in.p[0];
    pos.tran.y=p_in.p[1];
    pos.tran.z=p_in.p[2];
    double tmpRoll,tmpPitch,tmpYaw;
    p_in.M.GetRPY(tmpRoll,tmpPitch,tmpYaw);
    pos.a=180.0*tmpRoll/M_PI;
    pos.b=180.0*tmpPitch/M_PI;
    pos.c=180.0*tmpYaw/M_PI;

//    qDebug()<<"initial pose:"<<pos.tran.x
//                    <<pos.tran.y<<pos.tran.z<<pos.a<<pos.b<<pos.c;

    double joints[6];
    KINEMATICS_INVERSE_FLAGS  iflags;
    KINEMATICS_FORWARD_FLAGS  fflags;
    int tmpKey=hexpodKin->kinematicsInverse(&pos,joints,&iflags,&fflags);

    //1026.06,978.692,1039.101,1027.342,1015.565, 1076.142
    qDebug()<<"  kinematicsInverse result   "<<tmpKey<<"length:"<<joints[0]
                    <<joints[1]<<joints[2]<<joints[3]<<joints[4]<<joints[5];
    jointsOut.resize(6);
    for(int i=0;i<6;i++)
    {
        jointsOut(i)=joints[i];
    }
    return 1;

}



int CoordinateSolver::getFkPos_hexpod(const JntArray &q_in, Frame &p_out)
{
    EmcPose  pos;

    pos.tran.x=p_out.p[0];
    pos.tran.y=p_out.p[1];
    pos.tran.z=p_out.p[2];
    double tmpRoll,tmpPitch,tmpYaw;
    p_out.M.GetRPY(tmpRoll,tmpPitch,tmpYaw);
    pos.a=180.0*tmpRoll/M_PI;
    pos.b=180.0*tmpPitch/M_PI;
    pos.c=180.0*tmpYaw/M_PI;
    double joints[6];
    KINEMATICS_INVERSE_FLAGS  iflags;
    KINEMATICS_FORWARD_FLAGS  fflags;
    int tmpKey=hexpodKin->kinematicsForward(joints,&pos,&iflags,&fflags);
    switch(tmpKey)
    {
    case 0:
        return 1;
    case -1:
        addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7072,robotId);
        qDebug()<<"error,,,can not converge,111 !!!";
        return tmpKey;
        break;
    case -2:
        addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7073,robotId);
        qDebug()<<"error,,,can not converge,bias too big !!!";
        return tmpKey;
        break;
    case -5:
        addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7073,robotId);
        qDebug()<<"error,,,can not converge, exceed limits !!!";
        return tmpKey;
        break;
    default:
        addMsg(ENUM_MSG_ERROR,"CoordinateSolver","CoordinateSolver",7072,robotId);
        qDebug()<<"error,,,can not converge, unknow error !!!";
        return tmpKey;

    }

    qDebug()<<"  kinematicsForward result   "<<tmpKey<<"coordinate:"<<pos.tran.x
                    <<pos.tran.y<<pos.tran.z<<pos.a<<pos.b<<pos.c;
    return 1;
}

int CoordinateSolver::getIkPos_SIX(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
    //初始化中间变量
    double tmpPoseMatrix[4][4];
    double tmpBeforeJoint[6];
    double tmpJointSolution[6];

    tmpPoseMatrix[0][0]=p_in.M.data[0];
    tmpPoseMatrix[0][1]=p_in.M.data[1];
    tmpPoseMatrix[0][2]=p_in.M.data[2];
    tmpPoseMatrix[1][0]=p_in.M.data[3];
    tmpPoseMatrix[1][1]=p_in.M.data[4];
    tmpPoseMatrix[1][2]=p_in.M.data[5];
    tmpPoseMatrix[2][0]=p_in.M.data[6];
    tmpPoseMatrix[2][1]=p_in.M.data[7];
    tmpPoseMatrix[2][2]=p_in.M.data[8];

    tmpPoseMatrix[0][3]=p_in.p[0];
    tmpPoseMatrix[1][3]=p_in.p[1];
    tmpPoseMatrix[2][3]=p_in.p[2];
    tmpPoseMatrix[3][3]=1;
    tmpPoseMatrix[3][0]=0;
    tmpPoseMatrix[3][1]=0;
    tmpPoseMatrix[3][2]=0;


    for(int j=0;j<6;j++)
    {
       tmpBeforeJoint[j]=q_init(j);
    }
//     tmpBeforeJoint[1] -= M_PI_2;
//     qDebug()<<"tmpBeforeJoint[1]"<<tmpBeforeJoint[1];


    //求解
    SixJointRobot sixJointRobotSolver;
    sixJointRobotSolver.initialForIk(robotDhParameter);
    int tmpBest;
    int isIK = sixJointRobotSolver.getIkSolution(tmpPoseMatrix,tmpBeforeJoint,tmpJointSolution,0,tmpBest);
    if(1!=isIK)
    {
        qDebug()<<"get ik failed!getIkSolution";
        return isIK;
    }

    //求解值转换
    for(int i=0;i<6;i++)
    {
            joints(i)=tmpJointSolution[i];
    }

    //debug　将q_init，p_in，joints写入文件, 查看逆解数据
//    dataRecord->addItem();
//    for (int i = 0; i < 6; i++)
//    {
//        dataRecord->writeRecord(q_init(i));
//    }
//    for (int i = 0; i < 3; i++)
//    {
//        dataRecord->writeRecord(p_in.p.data[i]);
//    }
//    for (int i = 0; i < 6; i++)
//    {
//        dataRecord->writeRecord(joints(i));
//    }

    return isIK;

}

//int CoordinateSolver::setJointInitial(JntArray jointIn)
//{
//    jointInitial = jointIn;
//    return 1;
//}
int CoordinateSolver::mRad2mmDegree(vector<double> mRad, vector<double> &mmDegree)
{
    if(mRad.size()!=internalNum || mmDegree.size()!=internalNum )
    {
        qDebug()<<"mRad2mmDegree error,mRad.size()!=internalNum || mmDegree.size()!=internalNum ";
        return -1;
    }
    for(int i=0;i<internalNum;i++)
    {
        if((*robotJoints)[i].jointType==0)
        {
           mmDegree[i]= mRad[i]*180.0/M_PI;
        }
        else if((*robotJoints)[i].jointType==1)
        {
            mmDegree[i]=1000.0*mRad[i];
        }
    }
    return 1;

}

int CoordinateSolver::mRad2mmDegree(const JntArray& mRad, vector<double> &mmDegree)
{
    if(mRad.rows()!=internalNum || mmDegree.size()!=internalNum )
    {
        qDebug()<<"mRad2mmDegree error,mRad.size()!=internalNum || mmDegree.size()!=internalNum ";
        return -1;
    }
    for(int i=0;i<internalNum;i++)
    {
        if((*robotJoints)[i].jointType==0)
        {
           mmDegree[i]= mRad(i)*180.0/M_PI;
        }
        else if((*robotJoints)[i].jointType==1)
        {
            mmDegree[i]=1000.0*mRad(i);
        }
    }
    return 1;

}

int CoordinateSolver::mmDegree2mRad(vector<double> mmDegree, vector<double> &mRad)
{
    if(mmDegree.size()<internalNum )
    {
        qDebug()<<"mmDegree2mRad error,mmDegree.size()<internalNum";
        return -1;
    }
    mRad.resize(internalNum);
    for(int i=0;i<internalNum;i++)
    {
        if((*robotJoints)[i].jointType==0)
        {
           mRad[i]=mmDegree[i]*M_PI/180.0;
        }
        else if((*robotJoints)[i].jointType==1)
        {
            mRad[i]=mmDegree[i]/1000.0;
        }
    }
    return 1;
}

void CoordinateSolver::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotIdIn,
                                     int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;

    switch(messageCode)
    {
    case 7073:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(") hexpod 正解错误，不能收敛！！！").toStdString();
       infomationStr = str1;
       break;
    }
    case 7072:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(") hexpod 正解错误，未知！！！").toStdString();
       infomationStr = str1;
       break;
    }
    case 7071:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")类型").toStdString() +
               QString::number(parameter1).toStdString() +
               QObject::tr(". CoordinateSolver求解,轴数错误!").toStdString();
       infomationStr = str1;
       break;
    }
    case 7070:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")类型").toStdString() +
               QString::number(parameter1).toStdString() +
               QObject::tr("机型不支持迭代解法!").toStdString();
       infomationStr = str1;
       break;
    }
    case 7069:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(robotIdIn).toStdString() +
               QObject::tr(")类型").toStdString() +
               QString::number(parameter1).toStdString() +
               QObject::tr("机型不支持fut解法!").toStdString();
       infomationStr = str1;
       break;
    }
        case 7068:
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(robotIdIn).toStdString() +
                   QObject::tr(")类型").toStdString() +
                   QString::number(parameter1).toStdString() +
                   QObject::tr("机型不支持多解config!").toStdString();
           infomationStr = str1;
           break;
        }
        case 7067:
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(robotIdIn).toStdString() +
                   QObject::tr(")类型").toStdString() +
                   QString::number(parameter1).toStdString() +
                   QObject::tr("不支持位置逆解").toStdString();
           infomationStr = str1;
           break;
        }
        case 7066:
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(robotIdIn).toStdString() +
                   QObject::tr(")类型").toStdString() +
                   QString::number(parameter1).toStdString() +
                   QObject::tr("不支持速度正解").toStdString();
           infomationStr = str1;
           break;
        }
        case 7065:
        {
           string str1;
           str1 = QObject::tr("机器人(ID:").toStdString() +
                   QString::number(robotIdIn).toStdString() +
                   QObject::tr(")类型").toStdString() +
                   QString::number(parameter1).toStdString() +
                   QObject::tr("不支持位置正解").toStdString();
           infomationStr = str1;
           break;
        }

        case 7060://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")运动学位置逆解失败,位置或姿态不可达！").toStdString();
            infomationStr = str1;
            break;
        }
        case 7061://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")顶点奇异点逆解失败！").toStdString();
            infomationStr = str1;
            break;
         }
        case 7062://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")J3奇异点逆解失败！").toStdString();
            infomationStr = str1;
            break;
        }
        case 7063://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")J5奇异点逆解失败！").toStdString();
            infomationStr = str1;
            break;
        }
        case 7064://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")位置逆解，求解偏差过大！").toStdString();
            infomationStr = str1;
            break;
        }
        default:
        {
            qDebug()<<"CoordinateSolver::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}

int CoordinateSolver::checkIkSolverJointOffset(const JntArray& q_init, const JntArray& outputJoints)
{
    //限制１ms取点的关节变化小于9０度／s=3.14/2/1000rad/ms=0.00157=rad/ms
    // 不考虑速度，只管突变。突变必须小于４５ｄｅgree=0.785
    double allowOffset=0.785;//0.0157时，正常运行偶尔报错。
    //double allowOffset=3;
    int jointNumber=q_init.rows();
    // jointNumber=6;
    for(int i=0;i<jointNumber;i++)
    {
        float absResult=fabs(q_init(i)-outputJoints(i));
       if(absResult>allowOffset)
       {
           qDebug()<<"checkIkSolverJointOffset error！fabs(q_init(i)-outputJoints(i))= "<<absResult<<" i="<<i;
           for(int j=0;j<jointNumber;j++)
           {
               qDebug()<<"JointInitial"<<j<<":"<<q_init(j)<<";";
               qDebug()<<"outputJoints"<<j<<":"<<outputJoints(j)<<";";
           }
           return -1;
       }
    }
    return 1;
}

#endif



